#include "rclcpp/rclcpp.hpp"
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "std_msgs/msg/float32.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <base_interfaces/msg/armor.hpp>
#include <base_interfaces/msg/armors.hpp>
#include <base_interfaces/msg/gimbal_pose.hpp>
#include <base_interfaces/msg/big_yaw.hpp>
#include <base_interfaces/msg/kinematic_status.hpp>
#include <base_interfaces/msg/shooter.hpp>
#include <base_interfaces/msg/bt_aimer.hpp>
#include <base_interfaces/msg/debug_of_kinematic.hpp>
#include "libbase/common.h"
#include <chrono>
#include <mutex>

// 将角度约束在[-Pi, Pi]
#ifndef _std_radian
#define _std_radian(angle) ((angle) + round((0 - (angle)) / (2 * PI)) * (2 * PI))
#endif

// 状态量维度。取2或4，代表pitch和yaw上的位置，或位置与速度
#define _D_STATE_ 2
// 控制量维度。取2即可，代表pitch和yaw的速度
#define _D_CTRL_ 2

using std::placeholders::_1;
using std::placeholders::_2;

namespace wmj
{

    struct mpcParam
    {
        Eigen::Matrix<double, 4, 1> process_error_weight;   // 过程误差权重
        Eigen::Matrix<double, 2, 1> process_control_weight; // 过程控制权重
        Eigen::Matrix<double, 4, 1> target_error_weight;
    };
    /**
     * @brief PoseSolver类，用于位姿解算，并云台发布位姿
     */
    class PoseSolver : public rclcpp::Node
    {
    public:
        /**
         * @brief 构造函数
         * @param name 节点名字
         */
        PoseSolver(std::string name);

    private:
        std::map<int, int> model_armors;                                                 ///< 存储模型到装甲板数量的映射
        std::mutex mtx;                                                                  ///< 用于保护model指针的互斥锁
        rclcpp::TimerBase::SharedPtr timer_ptr_;                                         ///< 小云台控制timer
        rclcpp::TimerBase::SharedPtr big_yaw_timer_ptr_;                                 ///< 大Yaw控制timer
        base_interfaces::msg::KinematicStatus::SharedPtr status_ptr_;                    ///< model指针
        rclcpp::Subscription<base_interfaces::msg::KinematicStatus>::SharedPtr top_sub_; ///< ekf模型订阅者
        rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr bullet_speed_sub;        ///< 弹速回传
        rclcpp::Publisher<base_interfaces::msg::GimbalPose>::SharedPtr Gimbal_pose_pub_; ///< 云台位姿发布者
        rclcpp::Publisher<base_interfaces::msg::BigYaw>::SharedPtr Big_yaw_pose_pub_;    ///< 大Yaw发布者
        rclcpp::Publisher<base_interfaces::msg::Shooter>::SharedPtr shoot_pub_;          ///< 子弹发射发布者
        rclcpp::Publisher<base_interfaces::msg::DebugOfKinematic>::SharedPtr debug_pub;
        ///< debug使用，armors发布者，用于发布预测后的装甲板
        rclcpp::Publisher<base_interfaces::msg::Armors>::SharedPtr armors_pub;
        mpcParam m_mpc_param;
        rclcpp::Subscription<base_interfaces::msg::BigYaw>::SharedPtr Big_yaw_pose_sub_;

        std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr}; ///< tf监听器
        std::unique_ptr<tf2_ros::Buffer> tf_buffer_;                       ///< tf缓冲区

        double m_bullet_speed = 24;           ///< 子弹速度
        float last_bullet_speed = 0;          ///< last子弹速度
        std::vector<float> bullet_speed_list; ///< last子弹速度
        int slide_window_num = 2;

        std::string base_link_ = "base_link";         ///< 绝对系
        std::string shooter_link_ = "base_link";      ///< 电机系
        std::string shooter_pitch_link = "base_link"; ///< 云台系
        std::string shooter_yaw_link = "base_link";   ///< 云台系，但无pitch角
        bool fix_on_ = false;                         ///< 是否补偿
        bool m_track_center = false;                  ///< 是否（想要）锁中心
        bool m_enable_center_track = false;           ///< 是否能够锁中心
        float pitch_off_ = 0.0;                       ///< 俯仰偏移
        float yaw_off_ = 0.0;                         ///< 偏航偏移
        float m_timeout = 0.1;
        bool m_use_mpc = false; ///< 是否使用 mpc 的参数

        bool m_big_yaw_control = false; ///< 是否控制大Yaw
        bool m_enable_big_yaw_filter = false;
        float m_filter_alpha = 0.1;
        float m_filter_last_output;
        bool m_filter_init = false;

        float m_switch_threshold = 0.0; ///< 装甲板切换的角度差
        float m_time_off = 0.0;         ///< 预测时间
        float m_time_off_outpost = 0.0;
        float m_force_aim_palstance_threshold = 0.0;  ///< 强制允许发射的目标旋转速度最大值
        float m_aim_angle_tolerance = 0.0;            ///< 自动击发时目标装甲板绝对偏角最大值
        float m_aim_pose_tolerance = 0.0;             ///< 发射角度容忍度
        float m_aim_center_palstance_threshold = 0.0; ///< 锁中心的最低角速度阈值，角速度小于这个值不锁中心，弧度制
        float m_switch_trackmode_threshold = 0.0;     ///< 更换锁中心模式角速度阈值，弧度制
        float m_aim_center_angle_tolerance = 0.0;     ///< 锁中心，目标装甲板偏角判断

        bool debug = false; ///< 调试模式
        double last_time = 0;
        double shoot_interval = 0.0;
        double last_shooter_time = 0;
        float m_big_yaw = 0;

        bool keep_scan = true;

        int all_white_tolerance_stop_shoot_ = 8;
        int white_frame_ = 0;
        /**
         * MPC参数
         */
        int m_step; // 预测步长
        double m_dt;
        // 损失函数权重参数
        Eigen::Matrix<double, _D_STATE_, 1> m_process_error_weight;  // 过程误差权重
        Eigen::Matrix<double, _D_CTRL_, 1> m_process_control_weight; // 过程控制权重
        Eigen::Matrix<double, _D_STATE_, 1> m_target_error_weight;   // 目标误差权重

        // MPC系统状态矩阵
        Eigen::Matrix<double, _D_STATE_, _D_STATE_> m_A; // 状态转移矩阵
        Eigen::Matrix<double, _D_STATE_, _D_CTRL_> m_B;  // 控制输入矩阵
        Eigen::Matrix<double, _D_STATE_, _D_STATE_> m_Q; // 过程误差权重矩阵
        Eigen::Matrix<double, _D_CTRL_, _D_CTRL_> m_R;   // 过程控制权重矩阵
        Eigen::Matrix<double, _D_STATE_, _D_STATE_> m_F; // 目标误差权重矩阵

        // MPC中间计算矩阵
        Eigen::MatrixXd m_H;  // 二次项系数
        Eigen::MatrixXd m_E;  // 一次项系数
        Eigen::MatrixXd m_Ed; // 一次项系数（目标量）

        int m_state;
        bool m_aimer;
        bool m_shootable;
        // base_interfaces::msg::GimbalPose m_cur_pose;
        rclcpp::Publisher<base_interfaces::msg::GimbalPose>::SharedPtr mpc_vel_pub_; ///< 云台位姿发布者

        rclcpp::Subscription<base_interfaces::msg::BtAimer>::SharedPtr bt_sub; ///< 行为树bt订阅者
        std::atomic<int> enable_shoot;                                         ///< 是否能发射，由行为树控制
    private:
        void yaw_cb(const base_interfaces::msg::BigYaw &msg);

        /**
         * @brief bt回调函数
         * @param msg BtAimer消息
         */
        void bt_cb(const base_interfaces::msg::BtAimer &msg);

        /**
         * @brief 云台控制定时器回调函数
         */
        void control_callback();
        /**
         * @brief 大Yaw控制定时器回调函数
         */
        void big_yaw_timer_cb();
        /**
         * @brief 模型接收回调函数
         * @param status_msg ekf模型
         */
        void top_cb_(base_interfaces::msg::KinematicStatus::SharedPtr status_msg);

        /**
         * @brief 获取src_frame至tar_frame的坐标变换
         * @param src_frame 源坐标系
         * @param tar_frame 目标坐标系
         * @param stamp     时间戳
         * @param transform 返回的坐标变化
         * @return 是否成功进行坐标变化
         */
        bool getTransform(std::string src_frame, std::string tar_frame, builtin_interfaces::msg::Time stamp, geometry_msgs::msg::TransformStamped &transform);

        bool getCurPose(wmj::GimbalPose &pose);

        /**
         * @brief 获取云台角度
         * @param target_position   目标装甲板位置
         * @param bullet_speed      子弹速度
         * @param gimbal_pose       返回的云台角度
         * @return 是否成功获取云台角度
         */
        bool getAngle(geometry_msgs::msg::Pose target_position, float bullet_speed, base_interfaces::msg::GimbalPose &gimbal_pose);

        /**
         * @brief 获取装甲板
         * @param predict_time  预测时间
         * @param number        返回的装甲板数量
         * @return 装甲板
         */
        std::vector<base_interfaces::msg::Armor> getArmors(double predict_time, int number);

        /**
         * @brief 获取最近的装甲板位姿
         * @param predict_time      预测时间
         * @param armor_pose_out    返回的装甲板位姿
         * @return 是否成功获取装甲板位姿
         */
        bool getClosestArmorPose(double predict_time, geometry_msgs::msg::Pose &armor_pose_out);

        bool getFacingArmorPose(double predict_time, geometry_msgs::msg::Pose &armor_pose_out);

        /**
         * @brief 是否可以射击
         * @param target_armor_pose     目标装甲板位姿
         * @param target_gimbal_pose    目标云台位姿
         * @return 是否可以射击
         */
        bool shootable(geometry_msgs::msg::Pose target_armor_pose, base_interfaces::msg::GimbalPose target_gimbal_pose);

        void bullet_speed_cb(const std_msgs::msg::Float32 &msg);

        /**********MPC**********/
        void mpc_timer_callback();

        void setParam(mpcParam param);

        bool getGimbalSpeed(base_interfaces::msg::GimbalPose &target_gimbal_speed);

        bool getPredictControlTarget(Eigen::MatrixXd &predict_ctrl_target);

        bool getTargetPose(base_interfaces::msg::GimbalPose &pose, double time_off);

        void setMpcMatrix();

        Eigen::MatrixXd solveQuadProg(Eigen::MatrixXd H, Eigen::MatrixXd f);
    };
} // namespace wmj
